/*============================================================================*/
/*                          The Freescale Cup                                 */
/*============================================================================*/
/*                        OBJECT SPECIFICATION                                */
/*============================================================================*
* C Source:         apptask.c%
* %version:         1 %
* %created_by:      Continental %
* %date_created:     %
*=============================================================================*/
/* DESCRIPTION :  All tasks are called here                                   */
/*============================================================================*/
/* FUNCTION COMMENT : Add here your tasks, inside a periodic task             */
/* according to the desired execution period                                  */
/*                                                                            */
/*============================================================================*/
/*                               OBJECT HISTORY                               */
/*============================================================================*/
/*  REVISION |   DATE      |                               |      AUTHOR      */
/*----------------------------------------------------------------------------*/
/*  1.0      | 22/08/2011  |  Revision                     | Continental      */
/*                                                                            */
/*============================================================================*/

/* Includes */
/* ------------------- */
/* Common declarations */
/* ------------------- */
#include "apptask.h"

/*       SW API    */
/* --------------- */
#include "APPLeds.h"

#include "HALLeds.h"

/* Exported functions prototypes */
/* ----------------------------- */
void apptask_28us(void);
void apptask_28usd(void);
void apptask_56us(void);
void apptask_56us(void);
void apptask_pxRead(void);
void apptask_1ms(void);
void apptask_14ms(void);



/*Variables */
static T_ULONG pixels_camara[127];
static T_UBYTE LineArray[5];
bool capture_pixel= FALSE;
static T_UBYTE POSTN;

static T_ULONG minimo=0;
 
 
 T_UBYTE velocityMin = 170;
 T_UBYTE velocityMax = 180;    	//90 14 % ancho de pulso 
 T_ULONG mDerecho = 85;
 T_ULONG mIzquierdo = 85;
 
 
static T_UWORD direction1 = 611;		//  611 90 grados pocision del servo
 
T_UBYTE ScantLine_Start=50;// 64-14=50
T_UBYTE ScantLine_End=78;// 64+14=78
 
 
bool flatStart_ScantCam=FALSE;//usada para comensar el scaneo limitado de la camara  
bool i= FALSE;//counter for velocity control 
 //T_ULONG RESUL_ADC1=0;
 
/**************************************************************
 *  Name                 : apptask_28us
 *	ID					 : TASK_28US
 *  Description          : 
 *  Parameters           :  none
 *  Return               :	none
 *  Critical/explanation :  no
 **************************************************************/

 void apptask_28us(void){

	 			
 }
 
 /**************************************************************
 *  Name                 : apptask_28usd
 *	ID					 : TASK_1MS
 *  Description          : SI CAMARA
 *  Parameters           :  none
 *  Return               :	none
 *  Critical/explanation :  no
 **************************************************************/
 void apptask_28usd(void) {
		
	static T_UWORD lub_28us_counter =1;
 	inicio: 	
 	
 	/*Run every 40 ms (20ms task x 2)*/
 	if((lub_28us_counter ==2)||(lub_28us_counter ==3)){
 	 	
 	 		/*Toggle Led 1 every 40ms*/
 	 	
 		//APPLeds_Led1_app();
 		Set_Led1_State(LED_ON);
 	}
 	else{
 			Set_Led1_State(LED_OFF);
 	}
 	
 	if(lub_28us_counter == 599){
 		Set_Led1_State(LED_OFF);
 		lub_28us_counter= 1;
 		goto inicio;
 	}
 	
		 
	lub_28us_counter++;
 }
 
   /**************************************************************
 *  Name                 : apptask_56us
 *	ID					 : TASK_56US
 *  Description          :Signal CK CAMARA
 *  Parameters           :  none
 *  Return               :	none
 *  Critical/explanation :  no
 **************************************************************/
 void apptask_56us(void){
 	
 	static T_UWORD lub_28us_counter1 =1;
 	inicio: 	
 	
 	/*Run every 40 ms (20ms task x 2)*/
 	
 	if((lub_28us_counter1!=1)&&(lub_28us_counter1<=258)){
 	
 		/*Toggle Led 1 every 40ms*/
 		APPLeds_Led2_app();
 	}
 	else{
 			Set_Led2_State(LED_OFF);
 	}
 	
 	if(lub_28us_counter1 == 300){
 	
 		lub_28us_counter1= 1;
 		goto inicio;
 	}
 	
		 
	lub_28us_counter1++;
 
 
 

 }
 
    /**************************************************************
 *  Name                 : apptask_112us
 *	ID					 : TASK_112US
 *  Description          :pixel read
 *  Parameters           :  none
 *  Return               :	none
 *  Critical/explanation :  no
 **************************************************************/
 void apptask_pxRead(void){
 //128 pixeles por leer 
 //total de cuentas 150
	static T_UWORD pixel_112us_counter =1;//incrementa la posicion del pixel 
	static T_UWORD pixel_array_counter =0;//utilizado para el arreglo 
 	inicio: 	
 	
 
 	if((pixel_112us_counter!=1)&&(pixel_112us_counter<=258)&&((pixel_112us_counter%2)==0)){
 	
 		pixels_camara[pixel_array_counter]=(T_ULONG)(Read_adc_State());
 		pixel_array_counter++;
 	}
 	if( pixel_112us_counter ==259){
			 capture_pixel= TRUE;		 		
 	}
 	
 	if(pixel_112us_counter == 300){
 	
 		pixel_112us_counter= 1;
 		pixel_array_counter =0;
 		goto inicio;
 	}
 	
		 
	pixel_112us_counter++;
 

 		

 }
 
 
  
/**************************************************************
 *  Name                 : apptask_1ms
 *	ID					 : TASK_5MS
 *  Description          :
 *  Parameters           :  none
 *  Return               :	none
 *  Critical/explanation :  no
 **************************************************************/
 void apptask_1ms(void)
 {
	if((POSTN>=0)&&(POSTN<=60)){
		mDerecho=(T_UWORD)velocityMax+((POSTN-64)*4);
		mIzquierdo=(T_UWORD)velocityMax+((64-POSTN)*4);
		
	}if((POSTN>=68)&&(POSTN<=127))
	{
		mDerecho=(T_UWORD)velocityMax+((64-POSTN)*1);
		mIzquierdo=(T_UWORD)velocityMax+((POSTN-64)*1);
			
	}
	else
	{
		mIzquierdo=velocityMax;
		mDerecho=velocityMax;
	}
	
  
		initEMIOS_0chX(pin_PA6, mIzquierdo);//motor derecho 
		initEMIOS_0chX(pin_PA7, mDerecho);//motor izquierdo
	
		
 }
  		/*T_UBYTE velocityCal=0;
 		if((POSTN>=45)&&(POSTN<=81)){//if((POSTN>=42)&&(POSTN<=85))
 			if(i==FALSE){
 				i=TRUE;
 				mDerecho= velocityMax-40;
 				mIzquierdo= velocityMax-40;	
 			}
 		 	if(mIzquierdo< velocityMax)
 		 	{
 		 	
 				mIzquierdo+=2;
 				mDerecho+=2;	
 		 	} 	
 			
 		}
 		//izquierda
 		else if((POSTN>=0)&&(POSTN<=21))	{
 			mDerecho= velocityMax-60;//180 ;velocityMax-30
 			mIzquierdo=velocityMin/2;//velocityMin/2	
 			i=FALSE;
 			
 		}
 		else if((POSTN>=22)&&(POSTN<=44))	{
 			mDerecho= velocityMax-30;//velocityMax-20;
 			mIzquierdo=velocityMin-40;	
 			i=FALSE;
 			
 		}
 		//Derecha
 		else if((POSTN>=82)&&(POSTN<=104))	{
 			mDerecho= velocityMin-40;
 			mIzquierdo=velocityMax-30;//velocityMax-20	
 			i=FALSE;
 			
 		}
 		else if((POSTN>=105)&&(POSTN<=128)){
 			mDerecho= velocityMin/2;
 			mIzquierdo=velocityMax-60;//velocityMax-40
 			i=FALSE;
 		}
  
		initEMIOS_0chX(pin_PA6, mIzquierdo);//motor derecho 
		initEMIOS_0chX(pin_PA7, mDerecho);//motor izquierdo
		*/

 
 
 /**************************************************************
 *  Name                 : apptask_14ms
 *	ID					 : TASK_14MS
 *  Description          :
 *  Parameters           :  none
 *  Return               :	none
 *  Critical/explanation :  no
 **************************************************************/
	void apptask_14ms(void){
	 
	static T_UBYTE y=0;
	minimo= pixels_camara[5];
	 
	 	if(capture_pixel== TRUE){
			T_UBYTE x= 5;
			for(x; x<122;x++)//127
			{	
				if(pixels_camara[x]<minimo){
					minimo=pixels_camara[x];
					POSTN=x;
				}
	 		}				
	 	}
	 //camviar para el concurso
	 	
	 	
	 	setservo(POSTN);
		initEMIOS_0chX(pin_PA9, direction1);

 	
}

 /*if(capture_pixel== TRUE){ 
 	T_UBYTE ArrayScant_X= ScantLine_Start;
 	for(ArrayScant_X;ArrayScant_X<ScantLine_Start;ArrayScant_X++)
 	{
 		if(pixels_camara[ArrayScant_X]<minimo){
				minimo=pixels_camara[ArrayScant_X];
				POSTN=ArrayScant_X;
 		}
 	}
 }
 	
 	
 	if(POSTN<=14)
 	{
 		//POSTN =14;
 		ScantLine_Start=14;
 		ScantLine_End=43; 
 	}
 	if(POSTN>=113)
	{ 		
	 		ScantLine_Start=84;
	 		ScantLine_End=113;
	}
 	else {
 		ScantLine_Start=POSTN-14;
 		ScantLine_End=POSTN+14; 
 	}
 	

 
 if((POSTN>=50)&&(POSTN<=78))
 {
		setservoCenter(POSTN);
		mIzquierdo=	velocityMax;
		mDerecho=velocityMax;
		
 }
 if((POSTN>=0)&&(POSTN<=49))
 {
		setservo(POSTN);
		mIzquierdo=(T_UWORD)velocityMax+((64-POSTN)*3);
		mDerecho=(T_UWORD)velocityMax+((POSTN-64)*3);
			
 }
 if((POSTN>=79)&&(POSTN<=128))
 {
		setservo(POSTN);
		mIzquierdo=	(T_UWORD)velocityMax+((POSTN-64)*3);
		mDerecho=(T_UWORD)velocityMax+((64-POSTN)*3);
			
 }
	
 //camviar para el concurso
  	
	initEMIOS_0chX(pin_PA9, direction1);
 	initEMIOS_0chX(pin_PA6, mIzquierdo);//motor derecho 
	initEMIOS_0chX(pin_PA7, mDerecho);//motor izquierdo	
	*/

